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ABSTRACT 


This study aims at developing an improved methodology for position estimation 
by whieh sparse, erroneous and ineonsistent sensor observations ean be utilized for 
Autonomous Underwater Vehicle (AUV) terminal homing to an undersea docking 
station. 

An undersea docking station offers great potential for increasing AUV at-sea time 
and reducing survey costs. A key part of this system is the ability of the AUV to 
successfully dock. Part of the process for successful docking is AUV position estimation. 
The Digital Ultra-Short Baseline (D-USBL) is a sensor system used by the AUV to 
provide range and bearing measurements to the docking station. These measurements can 
be used by the AUV to improve its position estimate. 

Due to the nonlinearity of the D-USBL measurements, the Extended Kalman 
Filter (EKE), Unscented Kalman Filter (UKF) and forward and backward smoothing 
(FBS) filter were utilized to estimate the position of the AUV. After performance of these 
filters was deemed unsatisfactory, a new smoothing technique called the Moving Horizon 
Estimation (MHE) with epi-splines was introduced. The MHE smoothing filter used the 
dead reckoning measurements and the D-USBE measurements as constraints in the epi- 
splines optimization method. 

The analysis based on data sets of REMUS 100 AUV docking station runs was 
conducted using the MHE/epi-splines methodology and compared to EKE and UKF 
algorithms. The MHE/epi-splines algorithm demonstrated significantly better 
performance over the EKE and UKF. 
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I. 


INTRODUCTION 


A. MOTIVATION 

The autonomous underwater vehicle (AUV) has taken center stage in defense 
underwater surveys. AUVs are capable of doing more work over a short period while 
maintaining low operational costs. As of 2017, AUVs are used to conduct mine hunting 
operations, sea floor mapping and other important activities previously conducted by 
divers. In the civilian environment, AUVs are used mostly to inspect underwater pipes 
for maintenance purposes. Most civilian companies invest their resources into ensuring 
they have the best AUV with the latest equipment onboard to achieve their mission 
requirements successfully. The challenge facing the underwater robotics community is 
the cost of deploying and extracting these AUVs under potentially bad weather 
conditions. AUVs have limited operational duration due to their rechargeable battery 
capacity and their data storage capacity. An undersea docking station offers great 
potential for increasing at-sea time and reducing survey costs of AUVs. An undersea 
docking station provides the AUV’s battery recharging facility, data download and 
offload activities. The key part of using the undersea docking station is the ability of the 
AUV to dock. Once the AUV has docked in an undersea docking station, the batteries 
will be recharged and the data will be transferred to the user on the surface stations and 
the vehicle mission will be updated. The use of an undersea docking station permits the 
AUV to remain at sea for extended periods of time. 

B. PROBLEM STATEMENT 

The purpose of this study is to develop a methodology for position estimation by 
which sparse, erroneous, and inconsistent sensor observations can be used for AUV 
terminal homing to an undersea docking station. An undersea docking station offers great 
potential for increasing at-sea time and reducing survey costs of AUVs. A key part of this 
system is the AUV’s ability to successfully dock. Part of the process for successful 
docking is estimating the AUV’s position. The AUV’s digital ultra-short baseline (D- 
USBL) sensor system provides range and bearing measurements to the docking station. 
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The D-USBL range and bearing measurements are nonlinear, sparse, erroneous and 
inconsistent. 

Figure 1, courtesy of Horner and Mqana [1], shows a REMUS 100 AUV with a 
Woods Hole Oceanographic Institution (WHOI) docking station used by the Naval 
Postgraduate School (NPS)’s Center for Autonomous Vehicle Research (CAVR). The 
docking station is mounted on the ocean floor in the vicinity of the Monterey Inner Shelf 
Observatory in Monterey Bay, California. The image shows the REMUS 100 
successfully docked at the docking station. The docking station has a vertical USBL 
transponder affixed to the top, a big yellow cylindrical tank housing a 9.6 kWh battery 
pack, and a camera next to the USBL transponder. The vehicle is prohibited from 
traveling under the docking station by plexiglass, shown as the black object at the bottom 
of the image [1]. 



Figure 1. NPS REMUS 100 with WHOI Docking Station. Source: [I]. 

The AUV starts the terminal homing phase in an undersea docking station 

operation on the sea surface with access to Global Positioning System (GPS) 
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measurements. As the vehiele dives deeper underwater, it loses the GPS fixes and relies 
on the measurements received from dead reckoning sensors such as Doppler Velocity 
Log (DVL), Inertial Navigation System (INS), Depth sensor and Inertial Measurement 
Unit (IMU). These sensors have various sampling rates not exceeding lOHz and the 
position uncertainty grows linearly over distance travelled. In order to minimize the 
navigation error as the AUV begins the docking sequence, the AUV is installed with a D- 
USBL sensor on the bow and the docking station is installed with USBL transponder. 
The AUV initiates an interrogation of the docking station transponder and it continues to 
send a periodic sound pulse. When the AUV is within range it may receive a response 
and transform it to range and bearing. The range and bearing measurements are then used 
with dead reckoning measurements to update the position of the AUV. The challenge in 
position estimate for the AUV is how to properly incorporate these infrequent, potentially 
erroneous and inconsistent measurements into the position estimation filter. 

This study aims at developing a methodology that can utilize the sparse, 
erroneous and inconsistent USBL measurements to guide the AUV to an undersea 
docking station. The USBL range and bearing measurements are nonlinear in terms of the 
process used to compute them and in terms of their inconsistency. Due to nonlinearity of 
the USBL range and bearing measurements, the nonlinear position estimating filters 
such as Extended Kalman Filter (EKE), Unscented Kalman Filter (UKF) and 
forward and backward smoothing (FBS) are considered as possible solutions and 
evaluated using previous REMUS 100 AUV docking station runs. Additionally, a 
modified. Moving Horizon Estimation (MHE) position estimating method is developed 
and compared with the alternative methods. The MHE approach is novel due to the use 
of a non-linear least squares optimization approach known as epi-splines. Results 
between the methods are compared and contrasted using simulated and in-water results. 

C. LITERATURE REVIEW 

I. REMUS 100 Vehicles 

The REMUS 100 vehicle was first used for coastal monitoring missions, but 
recently, it has been used in many underwater operations by both defense and civilian 
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industries [2], REMUS 100 is an AUV designed to operate within 100 m of the oeean’s 
surfaee. REMUS 100 is eylindrieal in shape and portable in size, so it ean be earried by 
two persons. The modular design of the REMUS 100 ensures relatively easy 
interehanging of eomponents depending on mission requirements. 

Current designs eome with the latest teehnology to maintain high performanee 
and engineering quality. Although the size of the instrumentation has deereased, the 
vehiele’s enduranee and performanee has inereased. REMUS 100 vehieles were 
originally designed by the Woods Hole Oeeanographie Institution (WHOl) lab in the 
United States. The teehnology was further developed by Hydroid, Ine. and this eompany 
was eventually purehased by Kongsberg Maritime of Norway [3]. These vehieles are 
attraetive to users beeause they are relatively inexpensive and light-weight as well as 
easily operated by a trained operator via a standard laptop eomputer. 

2. Underwater Docking Stations 

In the past, underwater missions were short and quiek. Now, with advaneed 
teehnology, underwater missions ean be as long as the user desires. The new AUVs have 
better instrumentation, whieh produees high-quality results, and eonduet longer 
operations. The latest method for ensuring that the underwater environment is well- 
known by the users is to keep the AUV underwater and ready for the next operation. 
Underwater doeking stations help ensure that the vehiele is kept underwater with eharged 
batteries and with an updated mission plan. 

Underwater doeking stations are eonneeted to the base station by a 
eommunieation eable that allows the vehiele to reeharge its batteries and transfer data. 
Underwater doeking stations save time for deploying the vehiele and retrieving it after 
the mission. They also save time transferring data and loading the new mission. The 
underwater doeking stations also help to inerease the mission time. 

Most underwater doeking stations are tapered in shape, or funnel shaped, to eateh 
the eylindrieal vehiele when it misses the eenter of the doeking station. The eonieal shape 
ean eomplieate the doeking event beeause the AUV ean approaeh the doeking station 
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from only one direction unless the entrance cone pivots around a central axis, B. W. 
Hobson et al. [4], 

B. W. Hobson et al. [4], at the Monterey Bay Aquarium Research Institute 
(MBARI) developed and used the underwater docking station for docking the institute’s 
twenty one inch-diameter AUV. MBARI’s docking station is shown in Figure 2. This 
underwater docking station has had many successful docking events, which included 
transferring data, recharging batteries, and completing the power cycle of the AUV. 



Figure 2. MBARI AUV Docking Station. Source: [4]. 


Another interesting underwater docking station was designed by C. J. von Alt et 
al. [5]. The cone section of its underwater docking station is slightly different from the 
one shown in Figure 2. WHOI’s underwater docking station, like MBARI’s, can transfer 
data between the AUV and the home unit as well as recharge the AUV’s batteries. Figure 
3 shows the WHOI’s underwater docking station. 
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Figure 3. Complete AUV Doeking Station. Souree: [5]. 


M. D. Feezer et al. [6]. eonducted underwater doeking operations using an 
eleetromagnetie homing system to estimate the optimal position and orientation of the 
AUV. M. D. Feezer et al. installed a transmitter on the underwater doeking station and a 
reeeiver on the AUV. Figure 4 diagrams how the AUV interaets with the underwater 
doeking station when the eleetromagnetie homing system is used. 



Figure 4. Diagram of an Underwater Doeking Station with Eleetromagnetie 

Field Lines. Souree: [6]. 

The underwater doeking stations with eleetromagnetie homing have the disadvantage of 
having to handle unknown interferenee with nearby metallie objeets that produce 
additional magnetic fields. 
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Some docking stations are equipped with both optical homing systems and 
acoustic homing systems to maximize the chances of obtaining a successful docking 
event. This is a potentially useful approach in that the D-USBL system is used to guide 
the vehicle from farther away while the camera system can be used to provide final 
guidance commands. In this study, the underwater docking station is equipped with only 
a USBL transponder that communicates with the D-USBL sensor installed in the REMUS 
100 vehicle. 


3. Underwater Docking Environment 

As touched upon in the previous section, the undersea environment greatly 
impacts the sensor performance. Common sensor modalities include vision (optical) 
homing and acoustics homing methods. The vision homing method is known for its high 
update-rate advantage. However, the vision (optical) docking method requires relatively 
clear water visibility. This can be evaluated with the REMUS 100 through the optical 
backscatter sensor. Baratoff and Blansteen [7] found that the vision homing method can 
also be degraded by ambient light and infrared radiation. The vision homing method is 
only effective when the AUV is relatively close to the underwater docking station. 
Depending on water clarity it is limited to tens of meters. 

Figure 5 indicates the possible environment during underwater docking missions. 
The sea growth can obstruct the view of the AUV when docking using the optical homing 
method. 
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Figure 5. AUV Approaching the Underwater Docking Station. Source: [7]. 

If the acoustic homing method is used, the sea water needs to be quiet to allow for 
clear sound propagation between the transponder mounted in the docking station and the 
D-USBL in the AUV. While visibility does not negatively affect the acoustics homing 
method, proximity to the sea floor or subsea structures does. Although the acoustic 
homing method is affordable, it is known for low update rates. Figure 6 shows the 
MBARI underwater docking station which used an acoustic homing method to dock the 
twenty one inch Dorado AUV. 
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Figure 6. MBARI Underwater Doeking Station in Use. Source: [5]. 

In some cases, the vision and acoustic guidance methods are combined to improve 
the docking operation. The position of the docking station for this combination should be 
optimized to ensure that both methods produce usable results, A. Myagotin et al. [8]. 

4. EKF and UKF Position Estimating Methods 

This study evaluates the position estimating methods on using the nonlinear, 
sparse, erroneous and inconsistent USBL measurements to guide the AUV to an undersea 
docking station. Some studies are particularly relevant to this thesis. For instance, a few 
publications have covered the case of tracking underwater vehicles using D-USBL 
measurements and the optical guidance method. Filip Mandic et al. [9]. used D-USBL 
and sonar image measurements to track the underwater target precisely. Figure 7 depicts 
a small grayscale image captured by the forward looking sonar system. D-USBL 
measurements assist in bounding the uncertainties of the image to identify the object. 
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Figure 7. D-USBL and Sonar Image Positioning System. Source: [9]. 

The EKF was used to estimate the optimal position of the target from the 
availability of the target image in the sonar’s field of view. This study was conducted in 
two scenarios, one with a moving vehicle and slowly drifting target and one with a static 
vehicle and moving target. The results were as follows: the position estimate drifted 
every time the target was outside the sonar’s field of view and D-USBL measurements 
were unavailable. The fusion of the two sensors produced the most accurate tracking 
results in both scenarios. 

D-USBL measurements have become the common method for tracking the 
position of targets under water. N. Stilinovic et al. [10] conducted a study whereby the D- 
USBL was used to track a diver under water. An AUV that floated on the surface 
provided the diver below with correct GPS coordinates. N. Stilinovic et al. investigated 
“the guidance and control algorithms for the autonomous surface platform which enables 
diver tracking” [10]. In the study, the D-USBL was used to track the vertical position of 
the diver while the GPS was used for horizontal positioning. The LKF was used to 
provide the diver position estimates. The simulation yielded acceptable results except for 
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when the diver turned faster than the surfaee platform, thereby causing errors at the 
comers. 

Unlike the study conducted by Mandic et al. [9]., N. Stilinovic et al. [10] tracked 
the underwater moving object with D-USBL measurements only. The study by Mandic et 
al. [9]. is closer to the current study because measurements from two sources, sonar 
imaging and D-USBL measurements, were fused to obtain the optimal state of the 
underwater object. This thesis intends to apply a fusion of measurements from many 
AUV built-in sensors such as the DVL, depth sensor, INS, IMU and GPS as well as the 
D-USBL. The advantage of the current study is that the target is stationary and its 
position is known. 

0. Hegrenaes et al. [11] discovered another method of improving underwater 
navigation and control. Hegrenaes et al. discovered the method called underwater 
transponder positioning (UTP). This method integrates the AUV’s INS with the range 
measurements from the transponder. The UTP method is similar to the D-USBL but 
experiences better accuracy due to its tight integration with the INS. Hegrenaes et al. used 
the Kalman filter to estimate the optimal position of the transponders for this method. 
Hegrenaes et al. discovered that the UTP aiding method is possible and accurate for 
large-scale underwater operations [11]. Hegrenaes et al. study is related to this thesis 
because it used UTP-aided INS sensor measurements to bound the measurement errors. 
This thesis intends to bound the built-in sensor measurement errors using D-USBL fixes 
when they are available. 

Paul Frontera [12] conducted a study to estimate the state of the system with 
infrequently available measurement information. Frontera’s study aimed at estimating the 
submarine position without accessing the information from landmarks. Frontera focused 
on comparing the EKF’s and UKF’s performances in handling the nonlinear and sparse 
D-USBL measurements. Frontera discovered that the EKF and UKE show similar 
performances at short measurement intervals, but the UKE performs better at longer 
measurement intervals. These findings were expected as the UKE is adept at handling 
highly nonlinear measurements while the EKE uses second-order Taylor series 
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approximation, which fails at highly nonlinear measurements. This thesis intends to 
explore, through simulation, the EKF and UKF optimal state estimation methods. 

Another interesting use of the position estimating method appears in a study by 
Mohammed and Alzubaidi [13]. Mohammed and Alzubaidi used the position estimating 
method to traek an attaeking missile and prevent it from hitting its target using the EKF. 
Mohammed and Alzubaidi found that the estimated positions of the target were more 
aceurate when the target was eloser to the object. 

The position estimation method is also used in missile guided systems, as 
described in M. S. Beziek et al. [14]. Bezick et al. explored the “inertial navigation for 
guided missile using the position, veloeity and altitude for missile guidanee and eontrol 
and the inflight alignment teehnique that can be used to inerease the aecuraey of the 
missile navigation system data by ineorporating the external non-inertial navigation- 
aiding data using the Kalman filter and the measurements from the external radar traek of 
the missile and the GPS reeeiver” [14]. 

Feezer et al. [6]. used the eleetromagnetie homing system to bound the errors 
produeed by the AFTV’s dead reekoning systems when performing the underwater 
doeking operation. The eleetromagnetie homing method provides the AFTV’s bearing and 
orientation, whieh are very important for suoeessfully doeking, especially when the AFIV 
is elose to the doeking station. The eleetromagnetie homing system was found very 
effeetive at a range of 25 m to 30 m from the doeking station. This method ean also be 
used for multiple targets traeking at the same time. The traeking method in [6] works 
similar to the optical homing method explored by Mandic et al. [9]. Both methods 
provide good update rates but only at eloser ranges. 

N. Nadarajah et al. [15] eondueted a study to improve “the estimates for a multi 
target traeking problem using fixed-lag smoothing” [15]. Nadarajah et al. performed the 
fixed-lag traeking using forward filtering followed by baekward smoothing. The standard 
Kalman filter performed forward filtering in fixed-lag smoothing while the information 
filter performed backward smoothing. Although these filters use different sets of 
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measurements, they should eonverge into one optimal estimate. Sueh a method is 
explored in this thesis and may be integrated into the final guidanee algorithm. 

The EKF was also used to estimate the optimal position and attitude of spaeeeraft 
flying in formation. This study was eondueted by S. Kim et al. [16]. Given the study’s 
sueeess in traeking moving and stationery targets using position estimating methods, it is 
highly possible to guide an AUV to a stationary underwater doeking station using no GPS 
measurements and relying solely on sparse D-USBL fixes to bound the state estimation 
errors. 


5. Moving Horizon Estimation Position Estimating Method 

Moving horizon estimation (MHE) is the estimation method that eonsiders the 
eonstraints of vehiele dynamies while maintaining a minimum error between the estimate 
and aetual data. Although the MHE originated in the 1960s, it is now regarded as an 
important teehnique for position estimation for dealing with nonlinear data sampled at 
multisampling rates with outliers. The MHE has been applied in many different 
estimation seenarios by Royset and Wets [17]. 

The advantage of the MHE over the other estimation teehniques is that it uses 
eonstraints to ensure the estimation results are not influeneed by outlier measurements. 
This property forees the user to establish eonstraints from the beginning of MHE usage. 
M. W. Mehrez et al. [18] used the MHE to estimate the position of multiple robots when 
the initial estimate was unknown. Mehrez et al. ehose the MHE over the well-known 
EKE due to its advantage of working without knowing the initial estimate. The MHE 
performs a high number of iterations and eonverges more quiekly to the solution with the 
aeeepted error range. The EKE is highly affeeted by the initial conditions and may 
diverge. The study by Mehrez et al. found the MHE is unaffected by any kind of 
distribution. The challenge for the MHE, however, is solving linear programming at 
every time step. In Mehrez et al’s the study, the MHE performed better when the initial 
position estimate of the robot was unknown. The MHE uses the least squares 
approximation to overcome the nonlinearity of the measurements and to obtain prior 
knowledge to generate effective estimates when none are available. 
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The level of errors and rate of eonvergenee produeed by the MHE makes it 
attractive to the environment of noisy sensors and limited measurements in multi-rate 
systems. Andong and Wen-An [19] defines the MHE as “an optimization approach that 
produces estimates of states by using moving limited observed information.” Andong and 
Wen-An investigated the estimation performance of the MHE on a system that produces 
measurements at different sampling rates. The study employed an inertial sensor and 
camera to locate the position of a mobile robot. The MHE was used to merge the 
information from these two sources to improve the estimation results. 

The goal of the MHE is to minimize the objective function. The objective 
function comprises two parameters, the arrival cost and the stage cost. M. K. 
Ramalingam [20] defines the arrival cost as the uncertainty in the a priori estimate. 
Approximating the arrival cost is the main challenge for the MHE. The arrival cost is 
computed using nonlinear filters and passed to the MHE for optimization. If the nonlinear 
filter, such as the LIKE, produces a poor estimate of the arrival cost, the MHE’s initial 
optimization will also be poor. M. A. Muller [21] introduced the MHE scheme that 
confirms the assumption of prior weighting in the cost function offline. 

The MHE can be applied in many estimation environments. M. Zanon et al. [22] 
used the nonlinear version of the MHE to find estimates for the friction coefficient of 
autonomous vehicles travelling on the road. Zanon et al. used the direct multiple shooting 
method to find the estimated friction coefficient between the tires and the road. The study 
used least squares nonlinear programming as a mathematical tool to execute the 
computations in the MHE application. Zanon et al. also used the real-time iteration 
scheme with shifting since the computational effort for optimization is high for real-time 
feedback control purposes. 

According to Zanon et al. [22], “The real time iteration is also found to reduce the 
latency between the instant when the measurements are available and the instant when 
the feedback is available by preparing the computations as much as possible even before 
the measurements are available.” The MHE produced estimates for the friction 
coefficient in the study within milliseconds at the complexity of highly nonlinear models. 
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The MHE is sometimes used with epi-splines as a nonlinear least-squares eost- 
function minimization teehnique. 

Epi-splines were also used by Royset and Wets [17] in the artiele entitled “Erom 
Data to Assessments and Deeisions: Epi-Spline Teehnology.” Royset and Wets defines 
epi-splines as “the newly developed piecewise polynomial fimctions described by the 
finite number of parameters, which are exceptionally flexible.” As described in the 
article, epi-splines are used for image reconstruction, the probability density estimation of 
simulation output, as well as electricity demand forecasts with respect to weather 
conditions. In all of these applications, epi-splines were highly effective at predicting 
results. The effectiveness of the MHE depends on the mode of the probability density 
function [17]. 


6. Forward and Backward Smoothing Position Estimating Method 

The forward and backward smoothing is a technique applied to the measurements 
to obtain an optimal smoothed estimate, as described in D.J. Simon [23]. This technique 
results in two optimal estimates. The first estimate is obtained using an ordinary Kalman 
filter running forward in time from the first EISBE measurement to the EISBE 
measurement at the median of the measurements interval. The second estimate is 
obtained using an information filter running backward in time from the last measurement 
in the interval to the measurement at the median of the measurements interval. D.J. 
Simon [23] states that two estimates are unbiased because they are the outputs of the 
filters. The two estimates are combined according to their Kalman gains. 

The covariance of the forward smoothing step is initiated as a big finite value 
while the covariance for the backward smoothing step is infinite [23]. Since the infinite is 
not a valid number for computational purposes, the information filter is used for the 
inverse of the infinite covariance. The information filter uses zero as the confidence at its 
starting point. A zero confidence for the information filter means there is no knowledge 
of the state at the starting point. Eor computational purposes, the certainty of the 
information filter is initiated at small numbers greater than zero [23]. 
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The knowledge of the state at the information filter initialization point is key for 
better performanee of backward smoothing. Simon states that if the position of the object 
is unknown at the information filter starting point, the state’s initial condition should be 
zero, and the state will be updated only by observation and the Kalman gain. This could 
cause the information filter to diverge if the actual state is far away from the information 
filter starting position. 

D. STRUCTURE OF THE THESIS 

This thesis is divided into seven chapters. Chapter II provides an overview of the 
systems used in this thesis. Chapter III contains the problem description and simulated 
analysis of the UKF and EKF position estimating techniques. Chapter IV contains the 
detailed description of the Moving Horizon Estimation (MHE) with epi-spline algorithm. 
Chapter V presents the results from using UKE, EKE and MHE on previous REMUS 100 
docking station runs. Chapter VI contains a detailed description of the FBS position 
estimating method as well as analysis results whereby the EBS is evaluated and compared 
to the UKE, EKE and MHE position estimating techniques. Chapter VII concludes the 
paper. 
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II. SYSTEM OVERVIEW 


A. NPS REMUS 100 VEHICLE DESCRIPTION 

The NPS REMUS 100 vehicle has an aft propeller with two aft through-body 
thrusters (vertical and horizontal) and two forward through-body thrusters (vertical and 
horizontal). The NPS REMUS 100 vehicle was designed and manufactured by Hydro id and 
can operate to a maximum depth of 100 m. REMUS 100 vehicles are equipped with 
propellers and fins for moving forward and for diving. The main navigational sensors 
installed in the NPS REMUS 100 vehicles includes a GPS unit, Doppler Velocity Log 
(DVL), Acoustic Doppler Current Profiler (ADCP), depth sensor, Inertial Navigation 
System (INS), Inertial Measurement Unit (IMU) and other instruments as required by the 
user. One of the NPS REMUS 100 AUVs at the Center for Autonomous Vehicle Research 
is shown in Figure 8. This AUV is installed with standard REMUS 100 sensors to execute 
the underwater docking mission. The color segments indicate the modules used to make 
this AUV. 



Figure 8. One of the NPS REMUS 100 AUV 


When the vehicle is in water, it can use acoustic navigation to conduct an 
undersea survey in a given area. When the AUV gets underway, the sensors onboard start 
sampling and recording the data as the vehicle proceeds through the navigation 
objectives. The REMUS 100 has a control computer that guides the vehicle’s operation. 
REMUS 100 vehicles could replace divers in dangerous underwater operations. With the 

battery recharging and mission data-transfer capabilities of the underwater docking 
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stations, the REMUS 100 vehicles could spend more time and collect more meaningful 
data underwater at lower operating costs. Table 1 lists the physical specifications of the 
NFS REMUS 100 vehicle. 


Table 1. NFS REMUS 100 Vehicle Specifications. Source: [24]. 


Flatform 

REMUS 100 

Body Type 

Torpedo 

Body Size (L x W x H) 

2.22 mx0.19mx0.19m 

Hull Material 

Aluminum 

Weight 

52 kg to 64.5 kg in air 

Operational Depth 

3 m to 100 m 

Typical Search Area 

1200 m X 1000 m 

Dynamic Buoyancy 

No 

Self-Righting 

Yes 

Obstacle Avoidance 

No 

Endurance (Nominal Eoad) 

22 hrs at 1.5 m/s and 8 hrs at 2.6 m/s 

Fropulsion System 

Thrusters with direct drive DC brushless motors 

Degrees of Ereedom 

3 degrees of freedom controlled by fins 

Nominal Speed 

1.54 m/s 

Maximum Eorward Speed 

2.6 m/s 


The NFS REMUS 100 AUV is equipped with the built-in sensors and the modular 
sensors listed in Table 2. 


Table 2. NFS REMUS 100 Onboard Sensors 


Vehicle built-in sensors 

• GFS 

• Eong baseline (EBE) 

• Acoustic Modem 

• Kearfott INS 

• Side Scan Sonar 

• Acoustic Doppler Current Frofiler (ACDF)/Doppler 
Velocity Eog (DVE) 

• Conductivity Temperature Depth sensor 

• Optical Backscatter 

Vehicle modular sensors 

• Blueview Forward and Downward looking sonar 

• Downward looking Digital Video Recorder (DVR) 

• Digital Ultra Short Baseline (D-USBL) 
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Eric B. Bermudez [24] gives a full description of the NFS REMUS 100 vehicle 
equipment. The key component is the USBL hydrophone on the bow of the vehicle and 
the acoustic modem (located underneath just forward of the DVL). The D-USBL is an 
acoustic navigational aid for the REMUS vehicle, according to the REMUS/Swordfish 
Operations & Maintenance Manual [25]. The D-USBL consists of a potted assembly 
containing a four-channel planar hydrophone array with integral receiver/processor 
eircuit board. The REMUS vehicle uses D-USBL navigation during “transponder home” 
objectives, when it is closer to the transponder baseline, and sometimes when only one 
transponder is audible [3]. 

According to [3], during the D-USBL navigation, the acoustic modem transducer 
sends interrogation pings at three-second intervals. The D-USBL array receives the 
acoustic reply from any standard REMUS transponder in the field of view. To determine 
its position during a mission, the vehicle calculates the range and bearing to the 
transponder by measuring the difference in receive-times at each of the four hydrophones 
[3]. Figure 9 shows the four-channel planar hydrophone array. 



Digital USBL Array 


USBL 

Hydrophones 

■TOP’ 


Figure 9. D-USBL Array in Its Nose Cap with Four-Channel Planar 

Hydrophone Array. Source: [3]. 


For the vehicle to receive acoustic replies from the transponder mounted on the 
docking station, the transponder needs to be within +/-35 degrees of the D-USBL’s 
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maximum angle of expected transponder location. Figure 10 illustrates the relative 
positions of the vehicle and transponder for the best chances of receiving acoustic replies. 
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Figure 10. Cone Angle Specifications for D-USBL Measurements. Source; [3]. 


Table 3 details the performance specifications of the D-USBL from the REMUS/ 
Swordfish Operations & Maintenance Manual [3]. 


Table 3. D-USBL Performance Specifications. Source: [3]. 


Maximum angle from dead ahead 

+/- 35 degrees in Azimuth and in elevation 

Listening angle 

+/- 17.5 degrees in Azimuth and in elevation 
from the expected transponder bearing 

Maximum range 

2000 m 

Frequency range 

20-30 kHz 

Bearing accuracy 

+/- 2.4 degrees 

Bearing resolution 

+/- 0.25 degrees 

Range (travel time) accuracy 

+/- 1ms 

Range (travel time) resolution 

+/- 80 micro-seconds 
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The performance specification for the D-USBL is also illustrated in Figure 11. 


'+ 35' Beamwidth in Azimuth and Elevation 
^ ^ (max listening angle) 

/ 

/ 



Figure 11. D-USBL Performance Specifications. Source: [3]. 

B. NPS REMUS 100 DOCKING STATION DESCRIPTION 

The purpose of the underwater docking station is to provide a battery recharging 
facility for the AUV and to serve as an interface for data transfer between the vehicle and 
the user on shore. The NPS docking station was designed and manufactured by WFIOI 
and supplied to the Center for Autonomous Vehicle Research (CAVR). The docking 
station came with two ballast tanks that help lower and surface the docking station. The 
ballast tanks can be filled using a diver’s breathing apparatus. 

The NPS docking station is powered by a 9.6 kWh battery that is used to recharge 
the AUV’s batteries approximately 10 times and to supply power to other electronic 
components of the docking station. The NPS docking station is also equipped with a 
USBL transponder, which is used for terminal homing of the AUV. The docking station 
has a linear actuator with guide pins that help align the AUV after a successful docking 
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operation to successfully recharge the batteries and connect to the Ethernet cable for data 
transfer between the dock and the vehicle. 

The entrance to the docking station has a square-shaped opening of 0.7 m by 0.7 
m at the larger end of the funnel-shaped entrance. The smaller end of the funnel is 
circular with a diameter of 0.22 m and becomes the entrance for the cylindrical portion of 
the AUV when it is successfully docked. The length of the cylindrical opening is 0.93 m. 

The D-USBL transponder is mounted 0.42 m above and 0.15 m to the left of the 
cylindrical centerline, 0.56 m behind the square-shaped end of the funnel. The NFS 
docking station is also mounted with an underwater camera that helps the user to view 
and make necessary adjustments to the docking operation. The camera is cable-connected 
to the dock, which can be connected to a gateway buoy for wireless communication with 
users at the surface. Figure 12 shows the WHOI docking station which is utilized by the 
NFS CAVR in the vicinity of the Monterey Inner Shelf Observatory in Monterey Bay, 
California for undersea docking experiments. 
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Figure 12. WHOI Docking Station used by NFS CAVR in Monterey Bay, 

California. Source: [1]. 
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III. PROBLEM DESCRIPTION AND ANALYSIS 


The purpose of this study is to develop a methodology for position estimation by 
which sparse, erroneous, and inconsistent sensor observations can be used for AUV 
terminal homing to an undersea docking station. An undersea docking station offers great 
potential for increasing at-sea time and reducing survey costs of AUVs. A key part of this 
system is the AUV’s ability to successfully dock. This is a terminal homing problem 
where a robust solution addresses Guidance, Control and Navigation (GNC). Guidance is 
defined as the process of directing the AUV to a desired path or position. Control is 
defined as the process of maintaining the desired path or position using the feedback 
controller and considering the external factors. Navigation is defined as the process of 
localizing the vehicle’s position by using position estimation methods. Part of the process 
for successful docking is estimating the AUV’s position. The AUV’s D-USBL sensor 
system provides range and bearing measurements to the docking station. The D-USBL 
range and bearing measurements are nonlinear, sparse, erroneous and inconsistent. 

A. PROBLEM DESCRIPTION 

For the purposes of this thesis, the REMUS AUV will use a north-east-down 
(NED) coordinate system. Using a local reference frame, we define the following 
variables: longitudinal position (x), lateral position (y), downward position (z), surge 
velocity (u), sway velocity (v), heave velocity (w), roll angle (O), pitch angle (0), yaw 
angle (\|/), as described in T. I. Fossen [25]. These parameters are shown in Figure 13 at 
their corresponding axis of the AUV. The Inertial reference frame is illustrated by the 
capital letters X, Y and Z in North, East and Down directions, respectively. The down 
direction is illustrated by the broken lines in both Inertial and Eocal reference frames. 
The Local reference frame is illustrated by the small letter x, y and z directions which are 
on the body of the vehicle. The vehicle will also be controlled by the yaw-rate (r). 
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Figure 13. REMUS 100 AUV Axis and Control Parameters. Source: [25]. 

In this study, the simulated vehicle is an AUV following a set of waypoints and 
controlled by a cross-track-error controller. The AUV is released at a starting position 
closer to the first waypoint. The cross-track-error controller always attempts to keep the 
AUV on its intended track although the dead reckoning instruments cause it to deviate 
from the track as navigational errors increase with distance traveled. 

Figure 14 depicts the underwater setup as explained in this section. The vehicle is 
shown just past the first waypoint and approaching the second. The AUV track is shown 
as a red broken line while the intended track is shown as a black broken line. 
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Figure 14. AUV Docking Simulation Setup from Surface to 
Underwater Docking Station 


As the AUV follows the waypoints, the AUV can receive measurements at up to 
300m away from the USBL transponder mounted on the docking station. The USBL 
measurements received by the AUV are arbitrarily distributed and typically non- 
Gaussian. This makes it difficult to choose a position estimating technique that produces 
an accurate position estimate for the AUV. Wan and van der Merve [26] describes how 
the EKF assumes a Gaussian random variable (GRV) distributed state with a mean and 
variance and performs position estimation, which is accurate up to first-order Taylor 
series expansion. Given that the EKF can produce huge errors on highly nonlinear 
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systems, the UKF is also explored in this thesis. The UKF also assumes the state is a 
GRV distribution, but the UKF uses earefully ehosen sigma points to represent the actual 
state distribution and produces an error of at least second-order accuracy [26]. This 
terminal homing problem is nonlinear due to the fact that the state propagation equations 
(the system model) and the range and bearing measurements (the measurement model) 
are both nonlinear. 

Figure 15 shows how the EKF and UKF represent the state distribution. In this 
figure, the actual state is shown on the left with mean and covariance. The linearized 
EKF is shown in the middle with Gaussian approximation of the state while the 
unscented transform (UT), a UKF component, is shown on the right with five carefully 
placed sigma points to represent the actual state distribution. The true mean, as indicated 
by a small black circle, is shown at the bottom of each of these representations. As 
expected, the UKF locates its mean very close to the actual mean with a small variance. 
Meanwhile, the EKF mean and covariance (shown in red) differ noticeably from the 
actual mean and covariance (shown in black). 
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Figure 15. State Distribution as Represented by EKF and UKF (UT). 

Source: [27]. 


To further describe the challenge of selecting the best position estimation 
technique, Figure 16 depicts a high-level view of the AUV docking situation and the way 
the EKF and UKF represent the measurements. As shown, the UKF distribution can 
account for many USBE measurements due to the flexibility of placing the sigma points. 
The EKF is limited to the GRV distribution assumption and can only accommodate few 
measurements in its distribution. This causes the EKF to be less accurate on highly 
nonlinear measurements. In this figure, the AUV is shown approaching the region with a 
high concentration of USBE measurements. The AUV cannot use all measurements due 
to its dynamic limitations. The position estimation technique should, therefore, produce 
an optimal mean for the AUV to move in the right direction. 
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Figure 16. REMUS 100 AUV Docking Scenario and State Representation by 

UKF and EKF 

A general filtering approach is summarized in the fiow chart provided in Figure 
17. The fiow chart shows the measurement model on the left with the measured state. The 
measurements produced by the built-in AUV sensors such as GPS, INS and DVL are 
integrated into a single source of measurements with an update rate of not more than 10 
Hz. These measurements are then combined with the sparse, inconsistent and erroneous 
D-USBL measurements with update rate of about 0.2Hz to IHz. These measurements are 
combined in a data fusion process, which comprises the position estimation methods 
listed. The system model also provides the estimated state to the data fusion block, which 
fuses all measurements using the EKF or UKF with Kalman gain or by using the MHE 
with epi-splines. The data fusion results in the optimal estimated state, as illustrated on 
the right-end of the flow diagram. The optimal estimated state is then fed back to the 
system model as a new predicted state of the AUV in the next time step. If D-USBL 
measurements are not available, the vehicle uses the integrated system measurements to 
dead reckon to its future position. 
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Figure 17. General Data Filtering/Fusion Flow Chart 


B. DESCRIPTION OF OPTIMAL ESTIMATION USING EKE 

Rhudy and Gu [28] describe the EKF as an important technique in optimally 
estimating the state of engineering systems using an optimal observer gain to minimize 
the expected mean squared error of the estimate. The EKF assumes the linear Gaussian 
nature of the posterior state with mean and covariance to obtain an optimal solution. The 
EKF approaches the nonlinearity of the system by applying the analytical linearization, 
which involves using Jacobian matrices. The calculation of the Jacobian is a 
straightforward operation, but it has the potential for errors. 

The EKF is more effective in nonlinear systems up to the second degree of 
nonlinearity. The EKF becomes less efficient as the degree of nonlinearity increases. 
Even though the EKF is unlikely to be used on systems with higher levels of nonlinearity, 
it is simpler to apply and understand than the filters used for highly nonlinear systems. 

The main drawback for the EKF is that because solutions are restricted by the 
Gaussian assumption, highly nonlinear effects of the system are not included in the 
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variance estimate. This causes the EKF to perform poorly on highly nonlinear systems. 
Aeeording to Einieke and White [29], the EKE linearizes the nonlinear measurement 
model for its eurrent state estimate and uses the Kalman filter to propagate the state 
forward in time. 

The EKE uses five equations to obtain the optimal solution of the state estimation. 
The equations are as follows: 

Predietion Stage: 

System Model 


Xk\k-1 = /(4-1. Wfc_i) + Wfc Wfc = iV(0, cr) 

(1) 

Measurement Model 

Zfc = /i(4_i) + iV(0, cr) 

(2) 

Prior/Predietion Covarianee Matrix 

Pk\k-1 — ^k-lPk-l\k-l-^l:-l + Qk-1 

(3) 

Kalman Gain: 


Kk = Pk\k-iHliH„P„^„_^Hl + 

(4) 

Correetion Stage: 


Xk\k ~ -^kl/c-l T ^ki^^k ~ ^kXk\k-l) 

(5) 

Pk\k — Pk\k-lO ~ l^k^k) 

(6) 


In (1) is the new estimation at time k based on prediction obtained from 

time k-1, and is the input vector at time k-1 and is ignored for this study. Eunetion/ 
is a nonlinear eontinuous function; therefore, it is differentiable sueh that is a 

Jaeobian based on time k-1 obtained by differentiating the funetion / with respeet to the 
state as given by 

df 

(V) 

In (2) Z/j is the measurement model based on time k, h is the nonlinear funetion 
relating the current proeess state x^. to the eurrent measurement Z^, and is the 

measurement noise. In (3) P/c|fc-i is the eovarianee of the predieted state at time k based 
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on the prediction from time k-1, and Q^-i is the covariance of the linearized process 
model. 


In (4) K]^ is the Kalman filter gain at time k, while is the Jacobian obtained by 
linearizing the measurement function h around the current state as given by 


dh 

and is the covariance of the linearized measurement model. 


( 8 ) 


In (5) is the estimate of the mean at time k based on new measurements at 
time k and x^.|^._i, while in (6) is the covariance of the current estimate X/^j^.. 


In this study, the state transition matrix (A) is used to relate the state estimate to 
the new prediction. The equation showing the relationship between the state estimate and 
the current prediction is given by the commonly used and convenient first-order linear 
differential equation: 


X = i4x (9) 

the solution of which is given by 

0 = (10) 


where dt is the time increment in seconds. With this relationship, (1) and (2) become 


^k\k-l ^k^k-1 (11) 

Pk\k-1 = ^kPk-l^k 'k Qk-1 (12) 

The equations of motion of the vehicle underwater in NED coordinates, assuming 


roll angle is zero, are as follows: 


x^. = x^._i + ucos(9) cos(x(j) dt — vsin(xlj)dt 
Vk — y/c-i + ucos(d) sin(jp) dt -f vcos(jp)dt 

+ usin(6)dt 


The state is then given by 
dxk 


dt 


= X/j = ucos(^6) cosijp) — vsin(ilj) 


dyk 

dt 


= Yk = ucos(9) sin(x(j) -f vcos(x(j) 


(13) 

(14) 

(15) 


(16) 

(17) 
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dzj^ 

— = z^= usiniO) 


( 18 ) 


Therefore, the state transition matrix A is given by 


0 

0 

0 (cos(9) cos 

(V^)) -{sin(xp)) 

0 

0 

0 

0 

0 

0 

0 

0 

0 (cos(9 )sin 

(jp)) (cos(t/;))) 

0 

0 

0 

0 

0 

0 

0 

0 

0 

sin{6) 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 


And the matrix x is given by 

X 

y 

z 
u 

V 

w 
B 

r 

Range 
Bearing 

C. DESCRIPTION OF OPTIMAL ESTIMATION USING UKF 

The UKF is one of the most popular state estimation teehniques used for 
nonlinear systems. The UKF is from the same family of filtering systems as the EKF. The 
UKF is an improved method of filtering from the EKF. Sinee the EKF is known for 
produeing large errors in higher order systems, it is eonsidered a sub-optimal filter. The 
UKF, on the other hand, is eonsidered an optimal filter. 

The UKF employs the UT, whieh is used in ealeulating the statisties of a random 
variable undergoing a nonlinear transformation [26]. The UKF is aecurate up to third- 
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order Gaussian inputs for all nonlinearities and at least seeond-order for non-Gaussian 
inputs [26]. 

The UKF is an extension of the UT. The UKF is easier to apply than the EKF 
beeause it does not make any use of the Jaeobians, whieh may be ehallenging to evaluate 
and result in approximation errors [26]. The UKF employs deterministically-chosen 
weighted sample points known as sigma points to compute its version of the Jacobian and 
Hessian matrices without performing analytical differentiation. These sigma points are 
chosen to completely capture the true mean and covariance of current state variables. The 
UKF is accurate up to third-order Taylor series for any nonlinearity unless the prior 
random variable has a symmetric distribution such as an exponential family of probability 
density functions [26]. 


The following is the final algorithm used by the UKF to provide an accurate state 
estimate and its covariance. The UKF uses sigma points calculated as follows: 


A = + k) — L 


(19) 


where A is the scaling parameter, and a is the spread of the sigma points. The parameter 
a ranges between 10“^ and 1. When a is small, the sigma points get closer to each other, 
and when a is big, the sigma points scatter. The parameter L is the length of the state 
vector, while /c is a tertiary scaling parameter, which is equal to zero in most cases. This 
scaling parameter is used as a tuning “knob” to reduce estimation errors, as shown by 
Julier and Uhlmann [30]. The weighted vector for the mean (eta with superscript m for 
mean and subscript 0 for initial condition) is given by 




A 

L A 


( 20 ) 


The weighted vector for covariance (eta with superscript c for covariance and 
subscript 0 for initial condition) is given by 

+ P ( 21 ) 


In (21) (B is known as the secondary scaling parameter with an optimal value of 2 for 
Gaussian distributions. 
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1 


rir-vf 


2(L+X) 


fori — 1,... 2L columns 


( 22 ) 

(23) 

(24) 


+ vj'f 
=Vo+Vi 

The number of sigma points (n) depends on the length of the state vector, and the 
relationship is given by 

n = 2L + 1 (25) 

The size of the sigma point matrix is given by 

Xk-i = [4-14-1 + + ^ V4-1 4-1 - Vm V4-1 ] (26) 

In (26) 4-1 is the prior state estimate and is its covariance. is given by the 

Cholesky method Chol(P/(._i), where P^-i is the lower triangular matrix and k is the time 
of the computation. The sigma points are propagated through prediction using 

4|/c-i =/(4-i.Wfc_i) (27) 

In (27) i = 0,1,... 2L columns of the matrix, and / is the nonlinear function describing 
the dynamics of the system. Uj^-i is the vector containing the input parameters of the 
system. 

The mean is then predicted by 


2L 


4i/c-i = ^vrxi\k-i 


(28) 


1=0 


In (28) the weighted vector for the mean. 

The covariance of the predicted state is then given by 

2L 

Pk\k-i = Qk-1 {Xk\k-i - 4|k-i)(4|k-i -4|/c-i)’ 


(29) 


1=0 


In (29) Qii-i is the process error covariance matrix and ijf is the weighted vector for the 
covariance. 

The observation transformation is computed by propagating each sigma point 
through the observation as 

^k\k-i = f^0Ck\k-i>'^k) (30) 
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where i = 0,1,... 2L, and /i is a nonlinear observation function consisting of range and 
bearing nonlinear functions used in many underwater vehicle tracking algorithms. The 
mean predicted output is 

2L 

yk\k-i = '^vr^k\k-i ( 31 ) 

i=0 

where rj^is the weighted vector for the mean, and 4’k\k-i Is the propagation of sigma 
points. 

The covariance of the predicted output is given by 

2L 

+X “ yk\k-i){^i\k-i - fkik-iY ( 32 ) 

i=0 

In (32) is the measurement covariance. The cross-covariance of the state and the 
output is given by 

2L 

= ixi\k-i - 4|/c-i)Wi/c-i - yk\k-iY (33) 

i=0 

The measurement update stage of the UKF is similar to that of the Kalman filter 
and the EKF. The measurement update stage is given by 

=pyipyy' p4) 

In (34) is the Kalman gain, and by 

^k = ^k\k-l + l^ki^k ~ yk\k-l) (35) 

In (35) X/j is the state estimate update. The error covariance update is then given by 

Pk=Pk\k-i-KkPr^<k (36) 

D. SIMULATED ANALYSIS OF THE UKF AND EKF POSITION¬ 
ESTIMATING TECHNIQUES 

A simulation was conducted using the previous USBL measurements for the 
REMUS 100 AUV docking station. Dead reckoning measurements were simulated using 
the cross-track-error control and dead reckoning equations. A total of ten runs were used 
to evaluate the ability of the UKE and EKE in estimating the position of the vehicle. A 
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small bias associated with the heading was assumed and used for every run. The initial 
uncertainty of the vehicle with respect to the simulated USBL measurements was also 
assumed and used. 

The docking error was regarded as the distance from the center of the entrance of 
the docking station to the longitudinal centerline of the AUV. Figure 18 illustrates how 
the error was measured. In Figure 18 the vehicle is on the surface and the docking station 
is shown as a white rectangular frame. The reader should note that this is an illustration 
for docking error measurement only. For an AUV to dock, the error (e) should be less 
than 0.35m. The size of the square entrance is 0.7m in height and 0.7m in breath, and 
0.35m is the radius of the entrance. 



Figure 18. Docking Error Computation Illustration 


Table 4 shows the results of the simulation. No docking success was achieved in 
any run; however, as expected, the UKF managed to position the vehicle much closer to 
the docking station than the EKF technique. 
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Table 4. Simulated UKF and EKF Undersea Doeking Results 


Runs 

USBL 

Update rate 
in (Hz) 

Bias 

in 

(% distance 
travelled) 

Initial 

Uncertainty 
in (m) 

UKF 

Docking error 
(m) 

EKF 

Docking error 
(m) 

Docking 

Success 







REMUS 

UKF 

EKF 

1 

0.401 

0.70% 

10.8 

4.323 

4.331 

No 

No 

No 

2 

0.299 

1.00% 

8.5 

8.109 

8.129 

No 

No 

No 

3 

0.319 

0.90% 

11.4 

2.908 

2.911 

No 

No 

No 

4 

0.428 

0.60% 

7.5 

4.092 

4.102 

No 

No 

No 

5 

0.436 

0.70% 

6.7 

3.913 

3.924 

No 

No 

No 

6 

0.252 

0.60% 

4.2 

2.690 

2.698 

No 

No 

No 

7 

0.209 

0.40% 

3.0 

5.629 

5.651 

No 

No 

No 

8 

0.351 

0.60% 

3.0 

3.008 

3.018 

No 

No 

No 

9 

0.255 

0.40% 

3.3 

2.916 

2.925 

No 

No 

No 

10 

0.343 

0.50% 

3.0 

6.780 

6.801 

No 

No 

No 

Average 

0.329 

0.64% 

6.1 

4.437 

4.449 





Run 5 had a higher update rate; but the measurements were distributed poorly 
sueh that no doeking sueeess was possible using the UKF and EKF. Figure 19 shows the 
positions of the vehiele as represented by the curves associated with each estimating 
method. As expected, the UKF managed to position the vehicle a little closer to the 
docking station than the EKF. 


37 
























Intended track 
I I Starting point 
Docking station 
X USBL fixes 
I I Initial uncertainty 

- UKF 

- EKF 

- DR 


80 90 100 110 120 130 140 150 160 

East (m) 


Figure 19. Run 5 with Fligh Frequency Update Rate and Poor Docking Results 


Run 7 had the lowest frequency update of USBL measurements. The results 
obtained from this run were as expected, owing to the low USBL sensor update rate. 
Figure 20 provides an illustration of the measurements’ locations. As depicted, docking 
was impossible when the measurements were far from the intended track. 
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Figure 20. Run 7 with Lowest Update Rate and Poor Docking Results 
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IV. MOVING HORIZON ESTIMATION WITH EPI-SPLINES 


A. INTRODUCTION 

In the last chapter the EKF and UKF were used for AUV position estimation. The 
performance was poor due to the sparse, erroneous, sporadic USBL measurements. In 
this chapter the Moving Horizon Estimation is presented as an alternative. It is the state 
estimation method that uses ideally all available observed system dynamics 
measurements as constraints to estimate the state of a nonlinear or constrained system 
through solving an optimization problem [21]. The strength of the approach is that it 
handles the constraints in a more systematic way. Eurthermore a new, modified MHE has 
been developed. It leverages the fact that over relatively short time horizons the AUV 
trajectory has limited positional uncertainty. This can be combined with the USBE 
measurements to provide a more accurate, reliable position solution. The technique 
utilizes an epi-spline technique for non-linear least squares estimation of the moving 
horizon estimation. Epi-splines are “the newly developed piecewise polynomial functions 
described by the finite number of parameters, which are exceptionally flexible.” Royset 
and Wets [17]. They are useful for their ability to incorporate both soft and hard 
constraints Royset and Wets [31]. 

The EKE and UKE aimed to solve (37), the maximum likelihood estimate for the 
systems of equations (1) and (2). 

Xk = arg maxp(Xfc|zo,, z^) ( 37 ) 

where p(x/j|zo, —,Zk) is the probability that the system state is given measurements 
Zq, ...,z^.. As shown in Haseltime and Rawlings [32], the MHE maximizes the joint 
probability for the trajectory of the state values such that 

X;^, ....x^t =arg max p^x^, ...,Xt\zo, ...,Zk) (38) 

According to [32], for constrained nonlinear systems, logarithmic transformation 
of (37) is computationally easy: 

arg max -logp(Xfc, ...,X7.|zo, ....z^) = arg max p(Xfc, ...,X7.|zo, ....z^) (39) 
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because, by assuming that the a priori state estimate is a N(Xq,Y,o) distributed noise, 
minimization of (39), which is subject to the nonlinear model (1), gives rise to a least- 
squares optimization: 


T-l 


0r = min /Xxo) + > + > 

Xk,-,XT i—l i—l 

k=0 k=0 


^iRk^^k 


(40) 


Such that: iX^o) = (^o “ “ ^o) (41) 

Xk\k-1 = ^(4-1. Wfc_i) + Wfc (42) 

Zfc = /i(4_i) + (43) 

X eX, w ^W, V eV (44) 


Equations (40) through (43) are collectively called the full information problem, 
and X, W and V contain the values of the system state, system disturbance, and 
measurement disturbance, respectively [32], The X component contains values for the 
vehicle constraints such as turn rate, pitch rate, and velocity [32], This problem is 
difficult to solve due to its computational complexity. For this reason, the nonlinear least 
squares optimization technique is applied to solve the full information problem. 

In order to use the MHE approach for real-time applications, the optimization 
process should be quick and accurate; therefore, the epi-splines were used for the 
optimization process. Royset and Wets [33] derived the equations used for the epi-splines 
MATEAB programming code in this thesis. The equations used in this thesis were 
extracted from Horner and Mqana [1], Royset and Wets [17], [31] and [33], and Peter 
Tydingco [34], and they appear in the following paragraphs. The epi-splines are defined 
as the piecewise polynomials with extended, real valued, lower semi-continuous 
functions on R" and defined by the polynomial order, the number of partitions (S) and the 
mesh m = [!]• See Homer and Mqana [1] for an in-depth analysis of the epi- 

splines. 

During the optimization process, the epi-splines produce the vehicle’s trajectory 
from the vehicle state. The next step of the optimization process fits the vehicle trajectory 
to the USBE measurements accordingly [1]. The first step of the optimization process 
with epi-splines is described as follows: 
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(45) 



The epi-splines are piecewise polynomials at every segment with 
f{d) — representing the polynomial for vehicle data and 

Ij = |/(dt) “ representing the absolute value of the difference between the vehicle 
data and the predicted vehicle data. The constraints enumerated in (45) are used to 
minimize the average error between the vehicle data points and the fitted curve. The first 
and second constraints in (45) are for minimizing the error associated with the objective 
function. The third constraint is for ensuring continuity of the function. The fourth 
constraint is for ensuring continuity of the derivative of the function. The fifth constraint 
sets the upper and lower boundaries for the absolute difference between the vehicle data 
and the predicted vehicle data. The second step of the optimization minimizes the error 
between the USBL measurements and the curve fitted by the epi-spline optimization 
process. The resulting output is the MHE trajectory that represents the optimal estimate 
of the path of the vehicle. 


B. MOVING HORIZON ESTIMATION WITH EPI-SPLINE SIMULATION 

The MHE with epi-splines was applied to past docking station runs of the real 
REMUS 100 AUV. The AUV’s dead reckoning measurements were used as constraints 
for the epi-splines to produce the trajectory of the AUV. The nonlinear least squares 
method was then used to fit the trajectory of the AUV through the available USBE 
measurements in accordance with the covariance of the dead reckoning system and 
USBE measurements. 
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The MHE begins after the window has the first five USBL measurements. Figure 
21 illustrates how the MHE with epi-splines works. The MHE with epi-splines uses the 
measurements in the window and produces a curve for the estimated states associated 
with the measurements in the window. The MHE is applied only when an additional 
measurement has been obtained. The window is fixed to the first USBE measurement and 
extends every time there is a new USBE measurement. The process repeats until the epi- 
splines are applied in the window containing all USBE measurements. 



Every time the epi-splines produce the estimated means curve, the curve rotates 
using the covariance associated with the USBE measurements and the covariance 
associated with the dead reckoning measurements. 
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Figure 22 shows how the epi-splines with nonlinear least squares fit the curve for 
the means of the available USBL measurements and in accordance with the dynamics of 
the vehicle in the current window. 



Figure 22. Illustration of How the Optimal Curve Is Fitted 


Notably, the thick blue curve has the same trajectory as the curve for a dead 
reckoning AUV shown in Figure 22. While the covariance associated with dead 
reckoning measurements grows linearly over distance traveled, the covariance associated 
with the USBL measurements drops linearly as they get closer to the docking station. 

For this analysis, the vehicle started at a position just before it could receive the 
USBL measurements. During the complete run of the REMUS 100 AUV, the vehicle 
starts closer to the docking station and travels about 150 m dead reckoning before 
receiving USBL measurements. When the vehicle approaches the zone where it can 
receive USBL measurements, the accumulated dead reckoning error is high. This 
accumulated error is used in this thesis as the initial vehicle uncertainty. Sometimes the 
vehicle moves closer to the zone where the chances of receiving USBL measurements are 
high, making the initial uncertainty of the vehicle small. 
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The initial uncertainty is also used as the variance associated with dead reckoning 
measurements when rotating the epi-splines results according to the covariance 
associated with USBL and dead reckoning measurements. Figure 23 illustrates the 
rotation of the curves produced by the epi-splines for every window. The curves start by 
settling in between the dead reckoning and USBL measurements, but as the window 
stretches, the curves move away from the dead reckoning measurements and closer to the 
USBL measurements. 
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Figure 23. MATLAB Results Showing the Epi-Spline Optimal Solution in Every 

Window 

Figure 24 further illustrates the rotation of the curve for the estimated means and 
its deviation from the dead reckoning curve as the vehicle approaches the docking station. 
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C. MOVING HORIZON ESTIMATION WITH EPI-SPLINE ALGORITHM 

The algorithm used for the MHE is illustrated in Table 5. The inputs to the epi- 
splines algorithm are the MHE window size {MHE^g) , the system state for the window 
size ijCj), the covarianee for the system state of the window size (Pr), the constraints 
associated with vehicle dynamics as given by the measurement vector (Z^.), and the 
USBE measurements in the window size {Zusbl)- 

For accurate approximation results, the MHE horizon should contain ail the 
measurements; however, this requires significant computational memory from the system 
[21], The window size used for this thesis started with five EiSBE measurements, and it 
kept stretching to accommodate new measurements until all the USBE measurements fit 
within the window. 
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The methodology for propagating the vehiele from the starting position to the 
zone where USBL measurements are available is either the UKF or EKF. The MHE starts 
when the EiSBE measurements are available. The threshold is set to a small eonstant 
value. 


Table 5. Illustration of the Algorithm Used for the MHE with Epi-Splines 


Input 

Zx’ Pr’^k’^USBl’^^^ws 

Output 

Xepi’ ^epi 

Initialization 

Xq, Pq, MHE^g, jj., Threshold 

Computation of input 

Zp ~ [-^Tll-^Tl + ll 

parameters 

PT = [Pn\Pn+l\-Pk] 

^USBL — [^USBLn\^USBLn+l \ ■■■^USBLk] 

^k ~ l^nl^n+ll ■■■^kl 

nz = columns[Zk] 

Beginning the iteration 

with epi-splines 

if (nz > MHE^s) 

\zepv Pepi\ = epi_spline{zT’ Pr.Zk.ZusBi) 

if (// < Threshold) 

(.^k-N ’ ^k-N+l’ ■■■ ^k^ ~ Xgpi 

(.Pk-N’Pk-N + 1’ ■■■Pk)^ — Pepi 

else 

ZuSBL — [^USBLn+l\’ -"l^USBlk] 

end if 

end if 
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The optimization process takes place and provides the estimated state as long as 
the optimization error is less than the set threshold; otherwise, the bad measurement is 
left unused by the optimization, and the window extends to accept a new measurement. 
The optimization process continues until all USBL measurements have been used. The 
ability of epi-splines to incorporate soft and hard constraints makes them favorites in 
optimization problems. Other epi-spline advantages include their applicability in 
piecewise functions of any space, their requirement of one lower-semicontinuous input 
boundary, and their ability to approximate every lower-semicontinuous function to any 
accuracy. 
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V. RESULTS AND ANALYSIS 


A. METHODOLOGY 

The EKF, UKF and MHE with epi-splines were evaluated on their eapabilities of 
estimating the position of the REMEIS 100 AETV in the future using the current state of 
the system. Past docking mission runs of the AEIV were used to evaluate these position 
estimating techniques. To measure the effectiveness of each of these techniques, the final 
location of the REMUS 100 AUV was computed relative to the center of the entrance of 
the docking station. The distance from the center of the docking station entrance to the 
edge is 0.35 m in all directions. If the vehicle’s final position is within 0.35 m of the 
center of the entrance, docking is considered successful. The reader is referred to Figure 
18 for an illustration of the docking error metric. 

Sometimes the USBE measurements become available, and sometimes they do 
not. The USBE update rate is computed as follows: 


USBL Update Rate(in Hz) 


no. of USBL measurementsxaverage forward velocity 
distance between the docking station and the last waypoint 


In some runs, the update rate is very small, but the measurements are packed closer to the 
intended track. A total of 19 previous REMUS 100 AUV docking station runs were used 
for this evaluation. During these runs, the REMUS 100 AUV managed to dock only once 
during actual in-water testing. The position estimating techniques were evaluated to 
determine whether they would have managed to dock the vehicle by placing within the 
boundaries of the docking station entrance. The results for this evaluation are shown in 
the next section. 


B. PREVIOUS REMUS 100 DOCKING STATION RUNS - EVALUATION 
RESULTS AND ANALYSIS 

Figure 25 shows the REMUS 100 AUV’s dead reckoning trajectory for an 
underwater docking station run that was conducted in Monterey Bay, California in 2014. 
The vehicle attempted to dock during its first trial but missed the docking station. The 


51 



vehicle made its second attempt and docked successfully. Shown in the image is the 
combination of the two trajectories in blue. The red crosses represent the USBL 
measurements received during the first trial, and the black dots are USBL measurements 
received during the second trial. The vehicle started at the position marked in green as 
shown. The docking station was close to the starting point. The AUV’s intended track 
was the straight cyan line. Notably, the REMUS 100 AUV deviates from its path as the 
vehicle’s dead reckoning measurement error grows linearly. 
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Figure 25. MATLAB Plot of REMUS 100 Dead Reckoning for Two Trials 


The same illustration is shown in Figure 26. This diagram has been extracted from 
the REMUS 100 AUV software (VIP). 
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In Figure 26, the dead reckoning trajectory is shown in blue arrows; the yellow 
arrows indicate the USBL measurements available to the vehicle during the two trials. 
The docking station is indicated with the cyan star at the lower end of the intended track 
line. This docking station run corresponds with Runs 16 and 17 in Table 6. 



Figure 26. REMUS 100 Dead Reckoning Trajectory from VIP Softwear 

Table 6 shows the results of the evaluation conducted using previous docking 
station runs of the REMUS 100. As the table indicates, the MHE managed to dock the 
vehicle on five runs while the UKF and EKE only managed to dock the vehicle twice. 
The UKE and EKF docked the vehicle on the same run that the REMUS 100 managed to 
dock during its real mission in water. The MHE did not dock on this run, but it placed the 
vehicle close to the docking station. 
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Table 6. Results Obtained from Previous REMUS 100 Mission Runs Performed 

Using the UKF, EKF and MHE 


Runs 

USBL 

Update rate 

Initial 

Uncertainty 

UKF 

EKF 

MHE 

Docking 

Success 


(Hz) 

(m) 

Docking 
error (m) 

Docking 
error (m) 

Docking 
error (m) 

REMUS 

UKF 

EKF 

MHE 

1 

0.401 

20.80 

3.93 

4.65 

0.46 

No 

No 

No 

No 

2 

0.299 

16.00 

8.52 

9.22 

0.58 

No 

No 

No 

No 

3 

0.319 

22.40 

3.26 

4.51 

0.46 

No 

No 

No 

No 

4 

0.428 

15.80 

4.20 

5.40 

0.18 

No 

No 

No 

Yes 

5 

0.436 

13.70 

4.23 

4.87 

0.08 

No 

No 

No 

Yes 

6 

0.252 

8.20 

3.38 

3.84 

0.31 

No 

No 

No 

Yes 

7 

0.209 

2.50 

5.53 

6.32 

4.82 

No 

No 

No 

No 

8 

0.351 

0.00 

3.09 

4.01 

0.98 

No 

No 

No 

No 

9 

0.255 

2.60 

3.24 

4.41 

1.22 

No 

No 

No 

No 

10 

0.343 

0.00 

6.79 

7.20 

0.43 

No 

No 

No 

No 

11 

0.429 

3.70 

2.48 

3.38 

0.07 

No 

No 

No 

Yes 

12 

0.346 

6.80 

1.53 

2.76 

0.44 

No 

No 

No 

No 

13 

0.344 

4.60 

2.22 

2.96 

0.79 

No 

No 

No 

No 

14 

0.307 

5.80 

1.96 

3.35 

0.28 

No 

No 

No 

Yes 

15 

0.373 

8.10 

1.37 

3.20 

1.73 

No 

No 

No 

No 

16 

0.285 

9.10 

0.23 

0.28 

0.50 

Yes 

Yes 

Yes 

No 

17 

0.501 

7.50 

0.72 

0.53 

0.43 

No 

No 

No 

No 

18 

0.346 

15.10 

1.27 

0.07 

2.70 

Yes 

No 

Yes 

No 

19 

0.283 

20.00 

0.13 

0.50 

1.60 

Yes 

Yes 

No 

No 

Average 

0.336 

9.62 

3.06 

3.76 

0.95 






The average docking error associated with the MHE is relatively small compared 
to the EKF and UKF. 

In Run 2, the vehicle position was estimated very far from the docking station 
using UKF and EKF techniques, but the MHE located it about 0.58 m away from the 
docking station. The depiction of this run is shown in Figure 27. The UKF and EKF 
followed the position of USBL measurements with small consideration of the vehicle 
dynamics due to the tuning of the covariance associated with the system model and the 
covariance associated with the measurement model. 
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Figure 27. Run 2 Illustrating the Worse Performance by the UKF, EKF, and FBS 

Techniques 

In Run 3, the vehicle had high initial uncertainty about the USBL measurements, 
but the performance from all techniques was better than some of the runs with low levels 
of initial uncertainty. Figure 28 shows the performance of these techniques under the 
vehicle’s high uncertainty about the USBL measurements. 
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Figure 28. Run 3 Illustrating UKF, EKF and MHE Performance at a High Level 

of Initial Uncertainty 

In Run 4, the USBL sensor update rate is the highest for the runs during which the 
REMUS vehicle did not dock. Analysis finds that the vehicle could have docked if it had 
been guided by the MHE technique. Eigure 29 depicts this finding. 
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Run 14 had a low frequency update rate, but MHE technique managed to dock. 
Figure 30 illustrates this docking. Once again, the EKF and UKF tracked the USBL 
measurements with small consideration of the vehicle dynamics due to the tuning of the 
covariance associated with the system model and the covariance associated with the 
measurement model. 
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In Run 16, the REMUS vehicle docked, which means the USBL measurements 
were good and directed the vehicle into the docking station. This is confirmed by docking 
with the UKF and EKF because they are good at tracking the measurements. In this run, 
the MHE did not dock. This was expected because MHE considers the dynamics of the 
vehicle; this time, the MHE techniques failed, but managed to place the vehicle within 
0.5m from the center of the docking station. The equation used to combine the effect of 

the vehicle dynamics and the USBE measurements is as follows: 

2 2 

_ '^DRymhe <^USBLyDR ... 

yoptml mhe — r 

'-’DR '’USBL "dr "USBL 

In (46) yoptml mhe is the East (y) coordinate of the position of the vehicle as computed by 
the MHE with epi-splines method, is the uncertainty associated with the dead 
reckoning measurements at that specific location, ymhe is the output of the MHE with 

epi-splines method before considering the covariance associated with the dead reckoning 
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measurements and with the USBL measurements. In (46) is the East (y) eomponent 
of the dead reekoning position of the vehiele while (Jusbl is the eovarianee assoeiated 
with the USBL measurements. The eovarianee assoeiated with the dead reekoning 
measurements grows linearly with the distance travelled while the covariance associated 
with the USBL measurements decreases linearly as the vehicle gets closer to the docking 
station but cannot be zero because the USBL measurements normally end at about 5 m 
away from the docking station. 

As the vehicle moves closer to the docking station the yoptmi mhe tends to move 
away from the dead reckoning position of the vehicle due to (46). This becomes a 
challenge to the MHE especially when the USBL measurements are on the intended 
track; the MHE with epi-spline yoptmi mhe tends to be a little off from the intended track 
unlike the EKE and UKE. Eigures 31 and 32 provide the illustration of this situation. In 
this image, the measurements are on the intended track as a result, EKE and UKE 
managed to dock but MHE with epi-spline went a little off from the docking station. 
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In Run 19, the REMUS 100 AUV docked successfully. The UKF also managed to 
dock as expected. As discussed in Run 16’s explanation, the MHE with epi-splines 
missed the docking station by a small error. Figure 32 shows the distribution of the USBL 
measurements and the final estimated positions of the vehicle for other position 
estimating techniques. 
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VI. POSITION ESTIMATION USING FBS 


A. INTRODUCTION 

After evaluating the EKF and the UKF position estimating methods and obtaining 
unsatisfaetory results, the forward and baekward smoothing (FBS) position estimating 
method was introdueed. The FBS is a technique applied to the measurements to obtain an 
optimal smoothed estimate [23]. “The information filter is defined as the implementation 
of the Kalman filter that propagates the inverse of the covariance instead of propagating 
the covariance” [23]. Mathematically, the information filter is related to the Kalman filter 
by 

W = P~^ (47) 

In (47) is the certainty in the state for the information filter and P~^ is the inverse of 
the state covariance for the forward smoothing techniques [23]. The information filter is 
recommended for use in the backward smoothing technique where the knowledge of the 
state is uncertain or unavailable. This is due to the fact that the information filter is 
mathematically defined with zero confidence, = 0, where the uncertainty of the 
forward smoothing algorithm approaches infinity, ^ co [23]. The equations used to 
implement the backward smoothing as detailed in [23] are as follows: 

hnfl = Q-^ - Q-^0k (wl_i + (48) 

I inf'll is the certainty associated with the state at the previous time step. 

Q~^ is the inverse covariance for the process. 

0^. is the continuous state transition matrix as defined by 0 = 

is the certainty associated with the state at the previous time step. 

hnfl = hnfl + (49) 

hnf^ is the certainty associated with the state at one time step into the future. 

Hi is the transpose of the Jacobian obtained by linearizing the measurement function h 
around the current state. 
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R ^ is the inverse eovariance for the measurements. 



Kk = {linflY HIR-^ 

(50) 

K]^ is the Kalman gain. 





(51) 

xi^ is the predicted state. 




is the state at previous time step. 

4^ =4“+^k(^/c-W/c4") (52) 

is the smoothed estimate at one time step into the future. 


Z/j is the observed state. 


The FBS teehnique results in two optimal estimates. The first estimate is obtained 
using UKF running forward in time from the first to the fifth USBL measurements in the 
interval of ten measurements. The second estimate is obtained using an information filter 
running backward in time from the last USBL measurement in the current window to the 
fifth measurement in the current window size for the first nine USBL measurements. The 
two estimates are combined according to their Kalman gains and their covariances. The 
equations used are as follows: 

Pm = [(^/m)“' + (Pbmr^Y" (53) 

Pjn is the optimal smoothed estimate covariance. 

_ ^ 

(P/m) is the inverse of the forward smoothed estimate which is equal to 

(Pbm)~^ is the inverse of the backward smoothed estimate which is equal to 

~ PmQfm^fm 3” ^bm^bm) (54) 

is the optimal smoothed estimate. 


7)^ is the certainty of the forward smoothing technique. 


Ibm is the certainty of the backward smoothing technique. 
x^^ is the forward smoothed estimate. 
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is the backward smoothed estimate. 

The resultant smoothed estimate was utilized as an optimal estimate by an 
ordinary UKF that updated the state using only the smoothed estimates. The window of 
measurements expanded as new USBL measurements were received until all the 
available measurements were used to estimate the state of the system. Figure 33 
illustrates how the FBS was implemented. This image shows an uncertainty associated 
with the USBL measurements at the start of the region where the vehicle receives USBL 
measurements. The image also shows an uncertainty associated with the starting point of 
the backward smoothing as well as the red dot representing a smoothed estimate. The 
image also shows the window with 9 USBL measurements. Two REMUS 100 AUV 
positions are shown, representing a solution calculated from the dead reckoning 
measurements only, and the solution calculated from the FBS-smoothed estimates. 



The algorithm used for the FBS technique is illustrated in Table 7. In this table, N 
is the number of USBL measurements and m — N/2 is the stop position for both 
techniques. 
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Table 7. FBS Technique Algorithm 


Information Filter Input 

Xk'hnfi^ >Zk,N k = N:m 

UKF Input 

4|fc-i.Tfc|fc_i,Zfc,iV k = l:m 

Information Filter Output 

^bm> ^bm 

UKF Output 

1+ ^+ 

-^fm 

FBS output 


Information Filter initialization 


UKF initialization 

Xk-i,Pk-i,m 

Core steps 

for (k = 1: m) 

l^fm'^fm\ klKF (^Xk^k—lr Pk\k—1 > 

end for 

for (fe = N: m) 

Ubm’Xbm] = InfoJilter{Xk,Iinfl ,Zk,) 

end for 

Pfm = 

Pbm ~ Obm) 

Pm = [(PUT" + iPbmr^Y" 

~ PmOfm^fm "f ^bm^bm) 

Input to standard UKF 

^k\k-l’ Pk\k-1 'ZkDR 

Output from standard UKF 

Xk, Pk cmd Xyn, Pyn when available 


B. FBS EVALUATION AND COMPARISON WITH UKF, EKF AND MHE 
RESULTS 

The results associated with the FBS technique are shown in Table 8. The 
evaluation of the FBS was similar to the one conducted on the MHE with epi-splines. The 
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results obtained using the FBS are compared to the results obtained using the UKF, EKF 
and the MHE with epi-splines. It can be noted that the FBS managed to dock the AEiV 
five times as shown on the highlighted cells. The FBS performed much better than the 
UKF and EKF methods. 


Table 8. Docking Station Results for FBS in Comparison with UKF, EKF 

and MHE Methods 


Runs 

USBL 

Update rate 

Initial 

Uncertainty 

UKF 

EKF 

FBS 

MHE 

Docking 

Success 


(Hz) 

(m) 

Docking 
error (m) 

Docking 
error (m) 

Docking 
error (m) 

Docking 
error (m) 

REMUS 

UKF 

EKF 

FBS 

MHE 

1 

0.401 

20.80 

3.93 

4.65 

2.79 

0.46 

No 

No 

No 

No 

No 

2 

0.299 

16.00 

8.52 

9.22 

6.00 

0.58 

No 

No 

No 

No 

No 

3 

0.319 

22.40 

3.26 

4.51 

2.17 

0.46 

No 

No 

No 

No 

No 

4 

0.428 

15.80 

4.20 

5.40 

3.04 

0.18 

No 

No 

No 

No 

Yes 

5 

0.436 

13.70 

4.23 

4.87 

3.01 

0.08 

No 

No 

No 

No 

Yes 

6 

0.252 

8.20 

3.38 

3.84 

1.69 

0.31 

No 

No 

No 

No 

Yes 

7 

0.209 

2.50 

5.53 

6.32 

4.10 

4.82 

No 

No 

No 

No 

No 

8 

0.351 

0.00 

3.09 

4.01 

0.23 

0.98 

No 

No 

No 

Yes 

No 

9 

0.255 

2.60 

3.24 

4.41 

2.15 

1.22 

No 

No 

No 

No 

No 

10 

0.343 

0.00 

6.79 

7.20 

5.73 

0.43 

No 

No 

No 

No 

No 

11 

0.429 

3.70 

2.48 

3.38 

0.33 

0.07 

No 

No 

No 

Yes 

Yes 

12 

0.346 

6.80 

1.53 

2.76 

1.08 

0.44 

No 

No 

No 

No 

No 

13 

0.344 

4.60 

2.22 

2.96 

0.76 

0.79 

No 

No 

No 

No 

No 

14 

0.307 

5.80 

1.96 

3.35 

0.30 

0.28 

No 

No 

No 

Yes 

Yes 

15 

0.373 

8.10 

1.37 

3.20 

1.33 

1.73 

No 

No 

No 

No 

No 

16 

0.285 

9.10 

0.23 

0.28 

0.46 

0.50 

Yes 

Yes 

Yes 

No 

No 

17 

0.501 

7.50 

0.72 

0.53 

0.32 

0.43 

No 

No 

No 

Yes 

No 

18 

0.346 

15.10 

1.27 

0.07 

0.20 

2.70 

Yes 

No 

Yes 

Yes 

No 

19 

0.283 

20.00 

0.13 

0.50 

0.90 

1.60 

Yes 

Yes 

No 

No 

No 

Average 

0.336 

9.62 

3.06 

3.76 

1.93 

0.95 







The average docking error associated with FBS is 1.93 m which is much less than 
the docking error obtained using UKF and EKF methods. Meanwhile, the MHE 
algorithm successfully docked the vehicle as many times as the FBS algorithm did. 

Figure 34 is Run 2 in Table 8. This figure depicts a very bad run with badly 
distributed USBL measurements. The FBS performed better than UKF and EKF on this 
run. 
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Figure 34. Run 2 with Poorly Distributed USBL Measurements 


In Run 14, the FBS had a sueeessful doeking at a low USBL measurement update 
rate. Figure 35 illustrates the distribution of the position estimates obtained by the FBS 
teehnique and other methods analyzed during this run. 
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The FBS results demonstrate a system that does not entirely depend on the sparse, 
inconsistent and erroneous USBL measurements. The results indicate that FBS maintains 
a minimum covariance while also trying to reflect the vehicle dynamics. The strength of 
the FBS technique is the use of an optimal smoothed estimate to update the position of 
the AUV. The weakness associated with the FBS is the initialization requirement; one 
needs to have some knowledge of the actual position of the AUV in order to allocate the 
initial position for the backward smoothing process, since this method can fail to 
converge when initialized too far from the true initial position. Another weakness 
associated with the FBS is the fact that it does not use all the USBL measurements, this 
poses a risk of not using the best measurements. 
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VII. CONCLUSION AND FUTURE WORK 


A. CONCLUSION 

The purpose of this study was to develop a methodology for position estimation 
by whieh sparse, erroneous, and inconsistent sensor observations can be used for 
autonomous underwater vehicle (AUV) terminal homing to an undersea docking station. 

An undersea docking station offers great potential for increasing at-sea time and 
reducing survey costs of AUVs. A key part of this system is the AUV’s ability to 
successfully dock. Part of the process for successful docking is estimating the AUV’s 
position. The AUV’s D-USBL sensor system provides range and bearing measurements 
to the docking station. The D-USBL range and bearing measurements are nonlinear, 
sparse, erroneous and inconsistent. 

Position estimating methods, such as EKF and UKF were first considered and 
evaluated since the system is nonlinear in range and bearing measurements and these are 
estimation techniques for nonlinear systems. The evaluation results showed relatively 
poor performance for these filters. 

The MHE with epi-splines was then introduced as an alternative for position 
estimation for the AUV to an undersea docking station. The MHE smoothing filter used 
the dead reckoning measurements and the D-USBL measurements as constraints for the 
epi-splines optimization method. 

An analysis based on data sets of REMUS 100 AUV docking station runs was 
conducted using the MHE with epi-splines methodology and compared to EKE and UKE 
algorithms. The MHE with epi-splines algorithm demonstrated significantly better 
performance over the EKE and UKE. 

A further analysis with EBS was also conducted. The FBS technique was 
evaluated in the same manner as the MHE with epi-splines technique, and compared with 
UKE, EKE and MHE with epi-splines position estimating methods. The FBS method 
produced better results than the UKF and EKF methods, but the MHE remained the best 
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method; it produced consistently better docking results on real underwater docking 
datasets. 

The EKF had the highest average docking error of 3.76 m. The average docking 
error associated with UKF was 3.06 m. FBS technique had an average docking error of 
1.93 m while the MHE with epi-splines had an average error of 0.95 m. It was expected 
that the EKE would have the highest average error due to its performance limitations 
when applied to systems which are highly nonlinear. 

The lowest average docking error associated with the MHE indicates the great 
potential that the MHE has for guiding an AETV to an undersea docking station using 
sparse, erroneous and inconsistent EISBL measurements. 

B. FUTURE WORK 

The opportunity to evaluate all these position estimating techniques in a real 
underwater docking environment was not available for this thesis. Examining the MHE 
with epi-splines in a real underwater docking environment could produce many 
successful REMUS 100 AUV dockings. 

The possibilities of utilizing the UKF, EKF and FBS with a secondary in-close 
sensor system should be considered. The UKF, EKF and FBS results showed that they 
are good at tracking the USBE measurements, but these measurements (in many cases) 
end when the vehicle gets within 5 m of the docking station. As a result, these position 
estimating techniques only guide the AUV to a position about 5 m away from the 
docking station. A secondary in-close sensor system could provide a good solution to this 
problem. 

Investigation on when to ignore USBE measurements should be considered for 
future studies. Some USBE measurements lie far from the region where most USBE 
measurements are concentrated. These outliers cannot realistically reflect the actual 
vehicle position, due to the vehicle’s dynamic limitations. In this thesis, these 
measurements were utilized like other USBE measurements, but developing a method for 
eliminating spurious USBE measurements could produce better docking results. 
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Development of a minimal measurement eharaeteristie (in terms of sparsity, 
errors and ineonsistency of the USBL measurements) for sueeessful doeking should also 
be considered. Some sets of measurements are much more sparse, erroneous or 
inconsistent than other sets. Having a methodology for determining whether the USBL 
measurements are good or poor will assist in guiding the AUV since poor USBL 
measurements will be rejected. 
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